#include <FastSerial.h>
#define IMUPORT Serial1
#define DEBUGPORT Serial
#define BUFFERSIZE 32
FastSerialPort0(DEBUGPORT);
FastSerialPort1(IMUPORT);

#include <Servo.h>
Servo PITCHSERVO;
Servo ROLLSERVO;
#define ROLLPIN 6
#define PITCHPIN 7
#define ROLLMAX 2500
#define ROLLMIN -2500
#define PITCHMAX 4000
#define PITCHMIN -4000
#define CMDMAX 2100
#define CMDMIN 900

#define IMUPIN 8
#define LEDPIN 13

#define PANPIN 49

int IMUreceive(byte* buffer) {
  if (IMUPORT.available() >= 6) { //4prefix+size+ID
    if (IMUPORT.read()=='D') { //PRE1
      if (IMUPORT.read()=='I') { //PRE2
        if (IMUPORT.read()=='Y') { //PRE3
          if (IMUPORT.read()=='d') { //PRE4
            buffer[0]=IMUPORT.read(); //payload size
            if (buffer[0]>BUFFERSIZE-2){//if detected size is too large for buffer
              IMUPORT.flush();
              return 0;
            }
            buffer[1]=IMUPORT.read(); //Message ID
            while (IMUPORT.available()<buffer[0]);
            byte i;
            for (i = 2; i < buffer[0]+2 ; i++)
              buffer[i] = IMUPORT.read();
            IMUPORT.flush(); //ignore cksum
            return 1; //message available
          }
        }
      }
    }
  }
  return 0; //no message available
}

void setup()
{
  pinMode(LEDPIN, OUTPUT);  //LED debug
  pinMode(IMUPIN, OUTPUT); //groundstart
  DEBUGPORT.begin(57600);
  IMUPORT.begin(38400);
  ROLLSERVO.attach(ROLLPIN);
  PITCHSERVO.attach(PITCHPIN);
} 


int bytes2int(byte* buffer)
{
  union bytes2int {
    byte b[2];
    int i;
  } bytes2int;
  bytes2int.b[0]=buffer[0];
  bytes2int.b[1]=buffer[1];
  return(bytes2int.i);
}

long bytes2long(byte* buffer)
{
  union bytes2long {
    byte b[4];
    long l;
  } bytes2long;
  bytes2long.b[0]=buffer[0];
  bytes2long.b[1]=buffer[1];
  bytes2long.b[2]=buffer[2];
  bytes2long.b[3]=buffer[3];
  return(bytes2long.l);
}
  
void loop()
{
  //IMU msg variables   
  byte IMUbuffer[BUFFERSIZE];
  byte msgID;
  
  //IMU variables
  int roll;
  int pitch;
  int yaw;
  
  //Servo commands
  int rollcmd;
  int pitchcmd;
  
  //GPS variables
  long longitude=0;
  long latitude=0;
  int altitude=0;
  int ground_speed=0;
  int ground_course=0;
  long gpstime=0;
  byte fix;
  
  //Misc variables
  byte i;
  int light=LOW;
  unsigned long t=millis();
 
  digitalWrite(IMUPIN,LOW); //do a groundstart
  
  while(1){
        
    if(IMUreceive(IMUbuffer)){
      msgID=IMUbuffer[1];
      if (msgID==0x02){ //Attitude packet
        roll=bytes2int(&IMUbuffer[2]);
        pitch=bytes2int(&IMUbuffer[4]);
        yaw=bytes2int(&IMUbuffer[6]);
        
        roll=constrain(roll,ROLLMIN,ROLLMAX);
        pitch=constrain(pitch,PITCHMIN,PITCHMAX);
        
        rollcmd=map(roll,ROLLMIN,ROLLMAX,CMDMIN,CMDMAX);
        pitchcmd=map(pitch,PITCHMIN,PITCHMAX,CMDMIN,CMDMAX);
        
        ROLLSERVO.writeMicroseconds(rollcmd);
        PITCHSERVO.writeMicroseconds(pitchcmd);

        ///*
        //print values
        //DEBUGPORT.print(roll);DEBUGPORT.print("\t");
        //DEBUGPORT.print(pitch);DEBUGPORT.print("\t");
        //DEBUGPORT.print(rollcmd);DEBUGPORT.print("\t");
        //DEBUGPORT.print(pitchcmd);DEBUGPORT.print("\t");
        DEBUGPORT.println();
        //*/
      } else if (msgID==0x03){//GPS packet
        fix=IMUbuffer[20];
        if (fix) {
          longitude=bytes2long(&IMUbuffer[2]);
          latitude=bytes2long(&IMUbuffer[6]);
          altitude=bytes2int(&IMUbuffer[10]);
          ground_speed=bytes2int(&IMUbuffer[12]);
          ground_course=bytes2int(&IMUbuffer[14]);
          gpstime=bytes2long(&IMUbuffer[16]);
        }
        /*
        //print values
        DEBUGPORT.print(longitude);DEBUGPORT.print("\t");
        DEBUGPORT.print(latitude);DEBUGPORT.print("\t");
        DEBUGPORT.print(altitude);DEBUGPORT.print("\t");
        DEBUGPORT.print(ground_speed);DEBUGPORT.print("\t");
        DEBUGPORT.print(ground_course);DEBUGPORT.print("\t");
        DEBUGPORT.print(gpstime);DEBUGPORT.print("\t");
        DEBUGPORT.println(fix);
        */
      } else { //unknown packet
        DEBUGPORT.print("Invalid Packet");
      }//end packet detection
      
    } //end receive event
    
    if( (millis()-t) > 100) { // Main loop runs at 10Hz
      t=millis();
      light=!light;
      digitalWrite(13, light);   // set the LED on
    }//end 10Hz
    
  }//end while(1)
}//end loop()
